
# coding: utf-8
import matplotlib.pyplot as plt

P = 1.5
I = 0.1
D = 0.9

speed_want = 50 # the starting value
speed_now  = 200

err0 = 0
err1 = 0
err2 = 0

stop_count = 0
stop_flag = False

print ("PID")

arr = []
arr_ref = []

arr.append(speed_now)
arr_ref.append(speed_want)
for n in range(0, 200):
    err2 = err1
    err1 = err0
    err0 = speed_want - speed_now
    if ( (not stop_flag) and (err0 > -1) and (err0 < 1) ):
        stop_count = n
        stop_flag = True
    speed_now += P * (err0 - err1) + I * err0 + D * (err1 - err2)
    arr.append(speed_now)
    arr_ref.append(speed_want)

print (arr)

print ("stop_count: " + str(stop_count))

plt.plot(arr, linestyle='-')
plt.plot(arr_ref, linestyle='-')
plt.show()

